//
// Created by whitby on 8/22/23.
//

#ifndef MONO_ORB_SLAM2_G2OTYPES_H
#define MONO_ORB_SLAM2_G2OTYPES_H

#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/types/slam3d/se3quat.h>
#include "Sensor/Camera.h"

namespace mono_orb_slam2 {
    typedef Eigen::Matrix<double, 2, 1> Vector2d;
    typedef Eigen::Matrix<double, 3, 1> Vector3d;
    typedef Eigen::Matrix<double, 6, 1> Vector6d;

    class VertexPose : public g2o::BaseVertex<6, g2o::SE3Quat> {
    public:
        VertexPose() = default;

        VertexPose(const Eigen::Matrix3f &Rcw, const Eigen::Vector3f &tcw) {
            _estimate = g2o::SE3Quat(Rcw.cast<double>(), tcw.cast<double>());
        }

        bool read(std::istream &is) override { return true; }

        bool write(std::ostream &os) const override { return true; }

        void setToOriginImpl() override {
            _estimate = g2o::SE3Quat();
        }

        void oplusImpl(const double *update_) override {
            Eigen::Map<const Vector6d> update(update_);
            setEstimate(g2o::SE3Quat::exp(update) * estimate());
        }
    };

    class VertexPoint : public g2o::BaseVertex<3, Vector3d> {
    public:
        VertexPoint() = default;

        explicit VertexPoint(const Eigen::Vector3f &Pw) {
            _estimate = Pw.cast<double>();
        }

        bool read(std::istream &is) override { return true; }

        bool write(std::ostream &os) const override { return true; }

        void setToOriginImpl() override {
            _estimate.fill(0);
        }

        void oplusImpl(const double *update) override {
            Eigen::Map<const Vector3d> v(update);
            _estimate += v;
        }

    };

    class EdgeMonoPose : public g2o::BaseUnaryEdge<2, Vector2d, VertexPose> {
    public:
        EdgeMonoPose() = delete;

        explicit EdgeMonoPose(const Eigen::Vector3f &Pw) : camera(Camera::getCamera()) {
            P_w = Pw.cast<double>();
        }

        bool read(std::istream &is) override { return true; }

        bool write(std::ostream &os) const override { return true; }

        void computeError() override {
            const auto *vPose = dynamic_cast<const VertexPose *>(_vertices[0]);
            _error = _measurement - camera->project(vPose->estimate().map(P_w));
        }

        bool isDepthPositive() {
            const auto *vPose = dynamic_cast<const VertexPose *>(_vertices[0]);
            return (vPose->estimate().map(P_w))(2) > 0;
        }

        void linearizeOplus() override;

    private:
        Vector3d P_w;
        const Camera *camera;
    };

    class EdgeMono : public g2o::BaseBinaryEdge<2, Vector2d, VertexPoint, VertexPose> {
    public:
        EdgeMono() : camera(Camera::getCamera()) {}

        bool read(std::istream &is) override { return true; }

        bool write(std::ostream &os) const override { return true; }

        void computeError() override {
            const auto *vPose = dynamic_cast<const VertexPose *>(_vertices[1]);
            const auto *vPoint = dynamic_cast<const VertexPoint *>(_vertices[0]);
            _error = _measurement - camera->project(vPose->estimate().map(vPoint->estimate()));
        }

        bool isDepthPositive() {
            const auto *vPose = dynamic_cast<const VertexPose *>(_vertices[1]);
            const auto *vPoint = dynamic_cast<const VertexPoint *>(_vertices[0]);
            return vPose->estimate().map(vPoint->estimate())(2) > 0;
        }

        void linearizeOplus() override;

    private:
        const Camera *camera;
    };


} // mono_orb_slam2


#endif //MONO_ORB_SLAM2_G2OTYPES_H
